import matplotlib
import matplotlib.pyplot as plt
import scipy
from matplotlib.ticker import MultipleLocator
from mpl_toolkits.mplot3d import Axes3D
from scipy import interpolate
from scipy.optimize import leastsq
from scipy.interpolate import make_interp_spline
from PIL import Image
import numpy as np
import json
import cv2
from scipy.signal import argrelextrema
### 子函数：插值 #####################
from skimage import morphology


##################### 子函数：标准化 #####################
def MaxMinNormalization(list,maxValue,minValue):
    for i in range(len(list)):
        list[i] = (list[i] - min(list)) / (max(list) - min(list))*(maxValue-minValue)+minValue;
    return list

##################### 子函数：拟合函数，二次函数的标准形式 #####################

def quadraticFunction(params, x): #
    a, b, c = params
    return a * x * x + b * x + c


##################### 子函数：误差函数，即拟合曲线所求的值与实际值的差 #####################
def errorFunction(params, x, y):
    return quadraticFunction(params, x) - y

##################### 子函数：插值 #####################
def chazhi(sx, sy,mode):

    new_sx=[];
    new_sy=[];

    # 先对sx去重
    for i in range(0, len(sx)):
        if sx[i] not in new_sx:
            new_sx.append(sx[i]);
            new_sy.append(sy[i]);


    # 按sx排序
    Z = zip(new_sx,new_sy);
    Z = sorted(Z)  # 排列

    new_sx, new_sy = zip(*Z)


    # 生成插值坐标
    max_x = np.max(new_sx);
    min_x = np.min(new_sx);
    new_x = np.linspace(min_x, max_x,len(new_sx)*2);

    # 插值
    #fx=interpolate.interp1d(new_sx,new_sy,kind='cubic');
    #new_y=fx(new_x);


    if(mode==1):#样条插值
        new_y=make_interp_spline(new_sx,new_sy)(new_x);
    else:
        # 最小二乘法插值
        p0 = [10, 10, 10]
        Para = leastsq(errorFunction, p0, args=(np.array(new_sx),np.array(new_sy) ))
        new_y=quadraticFunction(Para[0], new_x);

    # fig4 = plt.figure()
    # plt.plot(new_sx, new_sy);
    # plt.plot(new_x, new_y);
    # plt.show();

    return new_x, new_y;

def fit(sx, sy, sz, step):
    new_sx = [];
    new_sy = [];
    new_sz = [];

    new_x = [];
    new_y = [];
    new_z = [];

    # 先对sy去重
    for i in range(0, len(sy)):
        if sy[i] in new_sy:
            sy[i]=sy[i]+0.00001;  #避免y值重复，无法拟合

        new_sx.append(sx[i]);
        new_sy.append(sy[i]);
        new_sz.append(sz[i]);

    # 插值
    last_kx = 0;
    kx = 0;

    for i in range(0, len(new_sy) - step, step):

        # 生成插值坐标
        max_y = np.max(new_sy[i:i + step]);
        min_y = np.min(new_sy[i:i + step]);
        y = np.linspace(min_y, max_y, step * 2);

        # fx=interpolate.interp1d(new_sy[i:i+3],new_sx[i:i+3],kind='quadratic');
        #
        # fz=interpolate.interp1d(new_sy[i:i+3],new_sz[i:i+3],kind='quadratic');
        # new_z.extend(fz(y));

        fx = np.polyfit(new_sy[i:i + step], new_sx[i:i + step], 1)
        px = np.poly1d(fx)

        fz = np.polyfit(new_sy[i:i + step], new_sz[i:i + step], 1)
        pz = np.poly1d(fz)

        last_kx=kx;
        kx = fx[0];
        kz = fz[0];

##################### 主函数 #####################
def start(input_file_name):
    sn = [];

    sx = [];
    sy = [];
    sz = [];

    sx_right = [];
    sy_right = [];
    sz_right = [];

    sx_left = [];
    sy_left = [];
    sz_left = [];

    sx_head = [];
    sy_head = [];
    sz_head = [];

    sx_tail = [];
    sy_tail = [];
    sz_tail = [];

    ####  读出所有数据行 ####

    file = open(input_file_name, encoding="utf-8")

    n = 0;
    while True:
        text = file.readline()  # 只读取一行内容
        if not text:  # 判断是否读取到内容
            break
        user_dic = json.loads(text)  # 每读取一行的末尾已经有了一个 `\n`

        sn.append(user_dic);

        for m in range(0, len(user_dic)):
            x = user_dic[m]['x'];
            y = user_dic[m]['y'];
            z = user_dic[m]['z'];

            sx.append(x);
            sy.append(y);
            sz.append(z);

        n = n + 1;

    file.close()


    #########  第二种拟合：采样生成二维图像再用像素点拟合 #########
    window = 20;  # 移动窗口

    # 采样
    sampling_interval = 0.5;  # 采样间隔

    max_x = max(sx);
    min_x = min(sx);

    max_y = max(sy);
    min_y = min(sy);

    max_z = max(sz);
    min_z = min(sz);

    sample_image_x = np.zeros(
        (int((max(sx) - min(sx)) / sampling_interval) + 1, int((max(sy) - min(sy)) / sampling_interval) + 1),
        np.float32);  # 平面投射采样点
    sample_image_y = np.zeros(
        (int((max(sz) - min(sz)) / sampling_interval) + 1, int((max(sx) - min(sx)) / sampling_interval) + 1),
        np.float32);  # 平面投射采样点
    sample_image_z = np.zeros(
        (int((max(sz) - min(sz)) / sampling_interval) + 1, int((max(sy) - min(sy)) / sampling_interval) + 1),
        np.float32);  # 平面投射采样点

    # 创建采样二值图
    for i in range(0, len(sy)):
        x = sx[i];
        y = sy[i];
        z = sz[i];
        sample_image_x[int((x - min_x) / sampling_interval)][int((y - min_y) / sampling_interval)] = sz[i];

    # 从平面图像计算轮廓
    '''
    img=np.array(sample_image_x,np.uint8)
    cv2.normalize(img, img, 0, 255, cv2.NORM_MINMAX)
    contours, hierarchy = cv2.findContours(img,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE);
    blank_image = np.zeros((sample_image_x.shape[0],sample_image_x.shape[1],3), np.uint8)
    cv2.drawContours(blank_image,contours,-1,(0,0,255),3) ;
    '''

    # 拟合
    '''
    fig1 = plt.figure()
    plt.subplot(2,1,1)
    plt.imshow(img,cmap='gray');
    plt.subplot(2,1,2)
    plt.imshow(blank_image);
    plt.show();
    '''
    #### 移动窗口拟合 ####

    kn = [];
    ka = [];
    kb = [];
    ki = [];
    fit_error = [];  # 拟合误差

    for n in range(0, sample_image_x.shape[1]):
        sx0 = [];
        sy0 = [];

        for i in range(0, sample_image_x.shape[0]):
            for j in range(n, n + window):
                if (i < sample_image_x.shape[0] and j < sample_image_x.shape[1] and sample_image_x[i][j] > 0):
                    sx0.append(i);
                    sy0.append(j);

        if (len(sx0) > 3):
            p0 = [0.1, -0.01, 100]  # 拟合的初始参数设置
            para = leastsq(errorFunction, p0, args=(np.array(sx0), np.array(sy0)))  # 进行拟合
            y_fitted = quadraticFunction(para[0], np.array(sx0))

            if (len(sx0) == len(y_fitted)):
                err = sum(abs(sy0 - y_fitted) ** 2) / len(sx0) * 5;
                fit_error.append(err);

                ka.append(para[0][0]);
                kb.append(para[0][1]);

                ki.append(abs(para[0][0] * para[0][1] * err * 2000));

                if (n < sample_image_x.shape[1] / 2):
                    kn.append(n);
                else:
                    kn.append(n + window);

    #### 计算鞋头鞋尾的位置 ####

    min_fit_error_tail = min(fit_error[0:20]);
    min_fit_error_head = min(fit_error[int(len(fit_error) - 20):len(fit_error)]);

    peaks_tail = argrelextrema(np.array(fit_error[0:20]), np.less_equal);
    peaks_head = argrelextrema(np.array(fit_error[len(fit_error) - 20:len(fit_error)]), np.less_equal);

    # 查找列表中最后一个满足条件的元素序号（鞋尾）
    m1 = 0;
    for i in peaks_tail[0]:
        if fit_error[i] < 150:
            m1 = i;

    # 查找列表中第一个满足条件的元素序号（鞋头）
    m2 = len(fit_error);
    for i in peaks_head[0]:
        if fit_error[i] < 150:
            m2 = i + len(fit_error) - 20;
            break;

    #### 计算所有横截面的鞋边点并生成采样图像 ####
    sx_contour = [];
    sy_contour = [];
    sz_contour = [];

    sample_image_contour = np.zeros(
        (int((max(sx) - min(sx)) / sampling_interval) + 1, int((max(sy) - min(sy)) / sampling_interval) + 1),
        np.uint8);  # 平面投射采样点

    m3 = 0;  # 鞋边缘点
    m4 = 0;
    shoe_width = 0;
    last_shoe_width = 0;

    for n in range(m1 + 5, m2):  # 处理鞋头鞋尾之间的横截面

        sx0 = [];
        sy0 = [];
        sz0 = [];

        for i in range(len(sn[n])):  # 横截面的点
            sx0.append(sn[n][i]['x']);
            sy0.append(sn[n][i]['y']);
            sz0.append(sn[n][i]['z']);

        sz01 = sz0[0:int(len(sz0) / 2)];
        sz02 = sz0[int(len(sz0) / 2):len(sz0)];

        peaks_section1 = argrelextrema(np.array(sz01), np.greater_equal);
        peaks_section2 = argrelextrema(np.array(sz02), np.greater_equal);

        for i in range(len(sz0)):

            if (sz0[i] == max(sz01)):
                m3 = i;
            elif (sz0[i] == max(sz02)):
                m4 = i;

        shoe_width = abs(sx0[m3] - sx0[m4]);

        if (last_shoe_width == 0 or shoe_width > last_shoe_width - 10):
            for m in [m3, m4]:
                x = sx0[m];
                y = sy0[m];
                z = sz0[m];
                sx_contour.append(x)
                sy_contour.append(y)
                sz_contour.append(z)
                sample_image_contour[int((x - min_x) / sampling_interval)][int((y - min_y) / sampling_interval)] = 1;

            last_shoe_width = shoe_width;

    # 滤波
    '''
    l=1;
    for i in range(l,sample_image_contour.shape[0]-l):
        for j in range(l,sample_image_contour.shape[1]-l):
            if np.array(sample_image_contour[i-l:i+l][j-l:j+l]).sum()<3:
                sample_image_contour[i-l:i+l][j-l:j+l]=0;

    '''

    s1 = sample_image_contour[:, m1:m1 + 10]
    s2 = sample_image_contour[:, m1 + 10:m2 - 10]
    s3 = sample_image_contour[:, m2 - 10:m2]

    s2 = morphology.remove_small_objects(s2, 64);

    sample_image_contour = np.concatenate((s1, s2, s3), axis=1)

    # 检测轮廓
    # cv2.normalize(sample_image_contour, sample_image_contour, 0, 255, cv2.NORM_MINMAX)
    contours, hierarchy = cv2.findContours(sample_image_contour, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_NONE);

    blank_image = np.zeros((sample_image_contour.shape[0], sample_image_contour.shape[1], 3), np.uint8)
    for c in contours:
        if (len(c) > 1):
            cv2.drawContours(blank_image, c, -1, (0, 0, 255), 3)

    # 把点列分成四个部分

    mean_x = np.mean(sx_contour);

    for i in range(len(sx_contour)):
        x = sx_contour[i];
        y = sy_contour[i];
        z = sz_contour[i];

        if i <= len(sx_contour) * 0.05:  # 鞋尾部分
            sx_tail.append(x);
            sy_tail.append(y);
            sz_tail.append(z);
        elif i >= len(sx_contour) * 0.95:  # 鞋头部分
            sx_head.append(x);
            sy_head.append(y);
            sz_head.append(z);
        elif x <= mean_x:  # 中段左侧
            sx_right.append(x);
            sy_right.append(y);
            sz_right.append(z);
        else:  # 中段右侧
            sx_left.append(x);
            sy_left.append(y);
            sz_left.append(z);

    # 插值
    # mode=1 样条插值

    # new_sy_right, new_sx_right = chazhi(sy_right, sx_right, mode=1)
    # new_sy_right, new_sz_right = chazhi(sy_right, sz_right, mode=1)
    #
    # new_sy_left, new_sx_left = chazhi(sy_left, sx_left, mode=1)
    # new_sy_left, new_sz_left = chazhi(sy_left, sz_left, mode=1)
    #
    # new_sx_tail, new_sy_tail = chazhi(sx_tail, sy_tail, mode=2)
    # new_sx_tail, new_sz_tail = chazhi(sx_tail, sz_tail, mode=2)  # mode=2  最小二乘法
    #
    # new_sx_head, new_sy_head = chazhi(sx_head, sy_head, mode=2)
    # new_sx_head, new_sz_head = chazhi(sx_head, sz_head, mode=2)

    '''
    new_sx_head.reverse();
    new_sx_left.reverse();

    new_sy_head.reverse();s
    new_sy_left.reverse();

    new_sz_head.reverse();
    new_sz_left.reverse();

    '''

    # 重新组装轮廓

    sx_contour = [];
    sy_contour = [];
    sz_contour = [];



    return sx_contour,sy_contour,sz_contour

    # w2f(sx_contour,sy_contour,sz_contour,out_put_file)

    #### 显示图案 ####
    # fig1 = plt.figure()
    #
    # plt.imshow(sample_image_x, cmap="gray");
    # plt.axvline(x=m1, ls="-", c="red", linewidth=2);
    # plt.axvline(x=m2, ls="-", c="red", linewidth=2);
    # plt.plot(fit_error, label='fit_error')
    #
    # ax = plt.gca()
    # x_major_locator = MultipleLocator(20)
    # ax.xaxis.set_major_locator(x_major_locator)
    # plt.legend()
    #
    # fig2 = plt.figure()
    # ax = Axes3D(fig2)
    # # ax.scatter3D(new_sx_head, new_sy_head, new_sz_head)
    # # ax.scatter3D(new_sx_left, new_sy_left, new_sz_left)
    # # ax.scatter3D(new_sx_tail, new_sy_tail, new_sz_tail)
    # # ax.scatter3D(new_sx_right, new_sy_right, new_sz_right)
    # ax.scatter3D(sx_contour, sy_contour, sz_contour)
    # ax.set_xlim(-100, 200);
    # ax.set_ylim(-100, 200);
    # ax.set_zlim(0, 100);
    #
    # fig3 = plt.figure()
    # plt.subplot(2, 1, 1)
    #
    # plt.imshow(sample_image_contour, cmap="gray");
    # plt.subplot(2, 1, 2)
    # plt.imshow(blank_image, cmap="gray");
    #
    # plt.show();
